#include <Arduino.h>
#include "SerialComs.h"
#include <EasyTransferI2C.h>
#include <Wire.h>

int inByte;

//create I2C Transfer Object 
EasyTransferI2C ET; 

//this is ths command structure for sending and receiving data between the master controller and nodes
struct SEND_DATA_STRUCTURE{
  //put your variable definitions here for the data you want to send
  //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
  char command; //command to execute on the slave node
  int data;    // data needed to execute the command, if any.
};
//give a name to the group of data
SEND_DATA_STRUCTURE i2cdata;


void setup_serialstuff()
{
		//Note: Serial0 on pins 3 (RX) and 2 (TX, Serial1 on pins 19 (RX) and 18 (TX) 
	//Serial2 on pins 17 (RX) and 16 (TX), Serial3 on pins 15 (RX) and 14 (TX).
	//Connect pin 14 (RX), pin 15(TX) to the TTL side of the RS232 chip
	//set comport3 on the 2560 Mega to 9600

	Serial3.begin(9600);
	Serial3.println("RoboStuff Version 1.0");
	Serial3.println("Developed by Rusty");
	Serial3.println("Waiting on Command...");

  Wire.begin();
  //start the library, pass in the data details and the name of the serial port. Can be Serial, Serial1, Serial2, etc.
  ET.begin(details(i2cdata), &Wire);

  //define handler function on receiving data
  Wire.onReceive(I2Creceive);


}

void ChkForCommand()
{
 //check for a comms from the pc
  if (Serial3.available() > 0) 
  {

    // get incoming byte:
    inByte = Serial3.read();
	//Serial3.print(inByte,1);
	  	switch(inByte)
		{
				case(0x31): //command 0x01 - forward
					i2cdata.command = 0x01;
					i2cdata.data = 128;
					ET.sendData(I2C_SLAVE_ADDRESS);
					Serial3.println("Forward");
				 break;

				case(0x32): //command 0x02 - Reverse
					i2cdata.command = 0x02;
					i2cdata.data = 128;
					ET.sendData(I2C_SLAVE_ADDRESS);
					Serial3.println("Reverse");
				 break;

				case(0x33): //command 0x03 - Turn left
					i2cdata.command = 0x03;
					i2cdata.data = 128;
					ET.sendData(I2C_SLAVE_ADDRESS);
					Serial3.println("Left");
				 break;

				case(0x34): //command 0x04 - Turn Right
					i2cdata.command = 0x04;
					i2cdata.data = 128;
					ET.sendData(I2C_SLAVE_ADDRESS);
					Serial3.println("Right");
				break;

				case(0x35): //command 0x05 - Stop
					i2cdata.command = 0x05;
					ET.sendData(I2C_SLAVE_ADDRESS);
					Serial3.println("Stop");
				 break;
		}
  }

}

void I2Creceive(int numBytes)
{
}